import sensor, image, time,pyb
from pyb import Pin, ExtInt,LED,Timer,UART
flag_key=0
line=0
flag_mode=100

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA2)
sensor.skip_frames(time = 2000)
uart = UART(3, 9600)
clock = time.clock()

pin0 = Pin('P0', Pin.IN)   #模式1
pin1 = Pin('P1', Pin.IN)   #模式2
pin2 = Pin('P2', Pin.IN)   #模式3
pin3 = Pin('P3', Pin.IN)   #模式4
pin7 = Pin('P7', Pin.IN)   #中断，（暂停程序）
pin8 = Pin('P8', Pin.IN)   #开始，（继续程序）

def callback_PIN7(line): #中断函数
    while(1):
        #duo_control_stop(50)
       # print(999999999)
        if pin8.value()==1:
            time.sleep_ms(50)
            if pin8.value()==1:
                print(999999999)
                break
extint = pyb.ExtInt(pin7, pyb.ExtInt.IRQ_FALLING, pyb.Pin.PULL_UP, callback_PIN7)

red_led   = LED(1)
green_led = LED(2)
blue_led  = LED(3)

text_Aend_x=0
text_Bend_y=0

Ax_n=0
center=[68,82]

def duo_control_stop():
    head=0x55
    data = bytearray([head , head,0x02,0x07])
    uart.write(data)
    time.sleep_ms(run_time)

def cmd_servo_move(num,run_time,servo_num,angle,servo_num1=0,angle1=0,servo_num2=0,angle2=0,servo_num3=0,angle3=0,servo_num4=0,angle4=0,servo_num5=0,angle5=0,servo_num6=0,angle6=0):
    head =0x55
    data_length = num*3+5
    cmd = 0x03
    if num == 1:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256])
        uart.write(data)
        time.sleep_ms(run_time)
    elif num == 2:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256, servo_num1 , angle1%256 , angle1//256])
        uart.write(data)
        time.sleep_ms(run_time)
    elif num == 3:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256, servo_num1 , angle1%256 , angle1//256, servo_num2 , angle2%256 , angle2//256])
        uart.write(data)
        time.sleep_ms(run_time)
    elif num == 4:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256, servo_num1 , angle1%256 , angle1//256, servo_num2 , angle2%256 , angle2//256, servo_num3, angle3%256 , angle3//256])
        uart.write(data)
        time.sleep_ms(run_time)
    elif num == 5:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256, servo_num1 , angle1%256 , angle1//256, servo_num2 , angle2%256 , angle2//256, servo_num3, angle3%256 , angle3//256, servo_num4, angle4%256 , angle4//256])
        uart.write(data)
        time.sleep_ms(run_time)
    elif num == 6:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256, servo_num1 , angle1%256 , angle1//256, servo_num2 , angle2%256 , angle2//256, servo_num3, angle3%256 , angle3//256, servo_num4, angle4%256 , angle4//256, servo_num5, angle5%256 , angle5//256])
        uart.write(data)
        time.sleep_ms(run_time)
def text_1():#复位
    cmd_servo_move(3,500,5,2000,6,1410)#5,水平，6，竖直
def text_2():
    cmd_servo_move(3,1500,5,2145,6,1413)#5,中——左（min,1657）
    time.sleep_ms(500)
    cmd_servo_move(3,1500,5,2145,6,1317)#5,中——上（min,1657）
    time.sleep_ms(500)
    cmd_servo_move(3,1500,5,1865,6,1317) #5,左上——右上（min,1657）
    time.sleep_ms(500)
    cmd_servo_move(3,1500,5,1865,6,1500) #5,右上——右xia（min,1657）
    time.sleep_ms(500)
    cmd_servo_move(3,1500,5,2145,6,1500) #5,右下——左下（min,1657）
    time.sleep_ms(500)
    cmd_servo_move(3,1000,5,2145,6,1413) #5,左下——左中（min,1657）
    time.sleep_ms(500)
def xy():
    numAx=0
    numBx=0
    numCx=0
    numDx=0
    numAy=0
    numBy=0
    numCy=0
    numDy=0
    num=0

    while(1):
        clock.tick()
        img = sensor.snapshot()
    # -----矩形框部分-----
        # 在图像中寻找矩形
        for r in img.find_rects(roi=(5, 26, 120, 100),threshold = 15000):
            # 判断矩形边长是否符合要求
            if r.w() > 40 and r.h() > 40:
                # 在屏幕上框出矩形
                img.draw_rectangle(r.rect(), color = (255, 0, 0), scale = 4)
                # 获取矩形角点位置
                corner = r.corners()
                #print(corner[0][0],corner[0][1])
                # 在屏幕上圈出矩形角点
                img.draw_rectangle((18, 26, 96, 96), color=(255,0,0))
                img.draw_cross(66,70,size=6)

                img.draw_circle(corner[0][0], corner[0][1], 5, color = (0, 0, 255), thickness = 2, fill = False)#左下
                img.draw_circle(corner[1][0], corner[1][1], 5, color = (0, 0, 255), thickness = 2, fill = False)#右下
                img.draw_circle(corner[2][0], corner[2][1], 5, color = (0, 0, 255), thickness = 2, fill = False)#右下
                img.draw_circle(corner[3][0], corner[3][1], 5, color = (0, 0, 255), thickness = 2, fill = False)#左上
                Ax=corner[3][0]
                Ay=corner[3][1]
                Bx=corner[2][0]
                By=corner[2][1]
                Cx=corner[1][0]
                Cy=corner[1][1]
                Dx=corner[0][0]
                Dy=corner[0][1]
                numAx=numAx+Ax
                numAy=numAy+Ay
                numBx=numBx+Bx
                numBy=numBy+By
                numCx=numCx+Cx
                numCy=numCy+Cy
                numDx=numDx+Dx
                numDy=numDy+Dy

                num=num+1
        if(num>=50):
             break
    Ax_n=numAx/num
    Ay_n=numAy/num
    Bx_n=numBx/num
    By_n=numBy/num
    Cx_n=numCx/num
    Cy_n=numCy/num
    Dx_n=numDx/num
    Dy_n=numDy/num

    A_Bx=(Ax_n+Bx_n)/2
    A_By=(Ay_n+By_n)/2
    B_Cx=(Cx_n+Bx_n)/2
    B_Cy=(Cy_n+By_n)/2
    C_Dx=(Cx_n+Dx_n)/2
    C_Dy=(Cy_n+Dy_n)/2
    D_Ax=(Dx_n+Ax_n)/2
    D_Ay=(Dy_n+Ay_n)/2



    print(Ax_n,Ay_n)
    print(Bx_n,By_n)
    print(Cx_n,Cy_n)
    print(Dx_n,Dy_n)
    print("end")
    return Ax_n,Ay_n,Bx_n,By_n,Cx_n,Cy_n,Dx_n,Dy_n,A_Bx,A_By,B_Cx,B_Cy,C_Dx,C_Dy,D_Ax,D_Ay
def Turn(Px,Py):
    dx=round((292/96)*(Px-18))
    Sx=2150-dx
    dy=round((185/96)*(Py-26))
    Sy=1310+dy
    return Sx,Sy
def text3():        #竖直放置
    listx=[]
    listy=[]
    list=xy()
    x0=list[0]*190/210
    x1=list[2]*193/210
    x2=list[4]*190/210
    x3=list[6]*190/210

    y0=list[1]*278/297
    y1=list[3]*278/297
    y2=list[5]*292/297
    y3=list[7]*292/297

    stra=Turn(x0,y0)
    print(stra)
    cmd_servo_move(3,3500,5,stra[0],6,stra[1])
    strb=Turn(x1,y1)
    strc=Turn(x2,y2)
    strd=Turn(x3,y3)
    for i in range(abs(strb[0]-stra[0])):
        cmd_servo_move(3,10,5,stra[0]+i,6,strb[0])
    for i in range(abs(strc[1]-strb[1])):
        cmd_servo_move(3,10,5,strb[0],6,strb[1]-i)
    for i in range(abs(strd[0]-strc[0])):
        cmd_servo_move(3,10,5,stra[0]-i,6,strc[1])
    for i in range(abs(stra[1]-strd[1])):
        cmd_servo_move(3,10,5,strd[0],6,strb[1]+i)


def kxy(beginx,beginy,endx,endy):
    Ka_b=(endy-beginy)/(endx-beginx)
    Ba_b=((beginy+endy)-Ka_b*(endx+beginx))/2
    num=abs(endx-beginx)
    num=round(num)
    print(num)
    if(beginx-endx>=0):
        for i in range (num):
            redx=beginx-i
            redy=round(Ka_b*redx+Ba_b)
            strx=Turn(redx,redy)
            #print(redx,redy)
            cmd_servo_move(3,20,5,strx[0],6,strx[1])
    elif(beginx-endx<0):
        for i in range (num):
            redx=beginx+i
            redy=round(Ka_b*redx+Ba_b)
            strx=Turn(redx,redy)
            #print(redx,redy)
            cmd_servo_move(3,20,5,strx[0],6,strx[1])
def kxy2(beginx,beginy,endx,endy):
    Ka_b=(endy-beginy)/(endx-beginx)
    Ba_b=((beginy+endy)-Ka_b*(endx+beginx))/2
    num=abs(endx-beginx)
    num=round(num)
    print(num)
    for i in range (num):
        redx=beginx-i
        redy=round(Ka_b*redx+Ba_b)
        strx=Turn(redx,redy)
        #print(redx,redy)
        cmd_servo_move(3,20,5,strx[0],6,strx[1])
def text4():
    listx=[]
    listy=[]
    list=xy()
    x0=list[0]*198/210
    x1=list[2]*195/210
    x2=list[4]*187/210
    x3=list[6]*180/210

    y0=list[1]*266/297
    y1=list[3]*297/297
    y2=list[5]*285/297
    y3=list[7]*270/297


    stra=Turn(x0,y0)
    print(stra)
    cmd_servo_move(3,500,5,stra[0],6,stra[1])
    time.sleep_ms(2000)

    kxy(x0,y0,x1,y1)
    time.sleep_ms(2000)
    kxy(x1,y1,x2,y2)
    time.sleep_ms(2000)

    kxy(x2,y2,x3,y3)
    time.sleep_ms(2000)
    kxy(x3,y3,x0,y0)
    time.sleep_ms(2000)

def text5():
    listx=[]
    listy=[]
    list=xy()
    x0=list[0]*180/210
    x1=list[2]*190/210
    x2=list[4]*190/210
    x3=list[6]*180/210

    y0=list[1]*273/297
    y1=list[3]*273/297
    y2=list[5]*292/297
    y3=list[7]*292/297

    stra=Turn(x0,y0)
    print(stra)
    cmd_servo_move(3,500,5,stra[0],6,stra[1])
    time.sleep_ms(1500)
    strb=Turn(x1,y1)
    print(strb)
    cmd_servo_move(3,500,5,strb[0],6,strb[1])
    time.sleep_ms(1500)
    strc=Turn(x2,y2)
    print(strc)
    cmd_servo_move(3,500,5,strc[0],6,strc[1])
    time.sleep_ms(1500)
    strd=Turn(x3,y3)
    print(strd)
    cmd_servo_move(3,500,5,strd[0],6,strd[1])
    time.sleep_ms(1500)
    cmd_servo_move(3,300,5,stra[0],6,stra[1])
    time.sleep_ms(1500)


def text4_huode():
    listx=[]
    listy=[]
    list=xy()
    x0=list[0]*180/210
    x1=list[2]*190/210
    x2=list[4]*190/210
    x3=list[6]*180/210

    y0=list[1]*273/297
    y1=list[3]*273/297
    y2=list[5]*292/297
    y3=list[7]*292/297

    stra=Turn(x0,y0)
    print(stra)
    cmd_servo_move(3,500,5,stra[0],6,stra[1])
    time.sleep_ms(1500)
    strb=Turn(x1,y1)
    print(strb)
    cmd_servo_move(3,500,5,strb[0],6,strb[1])
    time.sleep_ms(1500)
    strc=Turn(x2,y2)
    print(strc)
    cmd_servo_move(3,500,5,strc[0],6,strc[1])
    time.sleep_ms(1500)
    strd=Turn(x3,y3)
    print(strd)
    cmd_servo_move(3,500,5,strd[0],6,strd[1])
    time.sleep_ms(1500)
    cmd_servo_move(3,300,5,stra[0],6,stra[1])
    time.sleep_ms(1500)

def Mode_choose():
    while(1):
        clock.tick()
        img = sensor.snapshot()
        print(pin0.value(),pin1.value(),pin2.value())
        if (pin0.value()==0):
            flag_mode=1
            print(flag_mode)

            break
        elif (pin1.value()==0):
            flag_mode=2
            break
        elif (pin2.value()==0):
            flag_mode=3
            break
        elif (pin3.value()==0):
            flag_mode=4
            break
        else:
            flag_mode=9999
    return flag_mode

while(True):
    time.sleep_ms(1000)
    cmd_servo_move(3,500,5,2000,6,1410)#5,水平，6，竖直

    while(1):
        clock.tick()
        img = sensor.snapshot()
        print(pin0.value(),pin1.value(),pin2.value())
        text_1()
        Mode=Mode_choose()
        print(Mode)
        #print(Mode)
        if (Mode==1):
            text_1()                #复位
        elif(Mode==2):
            text_2()                #画大矩形
        elif(Mode==3):
            text3()                 #画竖直方向
        elif(Mode==4):
            text4()                 #画任意方向

